# -*- coding: UTF-8 -*-
'''
@Project ：microproduct1 
@File ：indirect_pstn.py
@Author ：SHJ
@Date ：2021/8/27 10:40 
@Vesion ：1.0.0
'''

import csv
import math
from oneOrthoAuxData import OrthoAuxData
from oneOrthoAlg import DirectPosition

def indirect_pstn(dem_resampled_path, meta_file_path):
    # 构建卫星轨道模型
    time_array, meta_data = OrthoAuxData.read_meta(meta_file_path)
    orbital_paras, T0 = DirectPosition.orbit_fitting(time_array, meta_data)

    # 获取DEM像元的地理坐标
    pstn_arr = OrthoAuxData.read_dem(dem_resampled_path, flag=1)
    print(pstn_arr.shape)
    y_size = pstn_arr.shape[0]
    x_size = pstn_arr.shape[1]

    # 从元文件读取一些参数
    starttime, endtime = OrthoAuxData.get_start_and_end_time(meta_file_path)
    cols, rows = OrthoAuxData.get_width_and_height(meta_file_path)
    λ = OrthoAuxData.get_lamda(meta_file_path)
    Td, doppler_d0 = OrthoAuxData.get_doppler_rate_coef(meta_file_path)
    # doppler_d = doppler_d0.tolist()
    doppler_d = [doppler_d0[0, 0], doppler_d0[1, 0], doppler_d0[2, 0], doppler_d0[3, 0], doppler_d0[4, 0]]

    for y in range(y_size):
        for x in range(x_size):
            Xp = pstn_arr[y, x, 0]
            Yp = pstn_arr[y, x, 1]
            Zp = pstn_arr[y, x, 2]
            Vpx = 0
            Vpy = 0
            Vpz = 0

            i = 0
            t = 0
            delta_t = 1
            delta_tr = (endtime - starttime) / cols
            iter_cnt = 0
            MaxIter = 1000

            while abs(delta_t) >= delta_tr and iter_cnt <= MaxIter:

                iter_cnt += 1

                Xs = orbital_paras[0][0] * t ** 3 + orbital_paras[0][1] * t ** 2 + orbital_paras[0][2] * t + orbital_paras[0][3]
                Ys = orbital_paras[1][0] * t ** 3 + orbital_paras[1][1] * t ** 2 + orbital_paras[1][2] * t + orbital_paras[1][3]
                Zs = orbital_paras[2][0] * t ** 3 + orbital_paras[2][1] * t ** 2 + orbital_paras[2][2] * t + orbital_paras[2][3]
                Vsx = orbital_paras[3][0] * t ** 3 + orbital_paras[3][1] * t ** 2 + orbital_paras[3][2] * t + orbital_paras[3][3]
                Vsy = orbital_paras[4][0] * t ** 3 + orbital_paras[4][1] * t ** 2 + orbital_paras[4][2] * t + orbital_paras[4][3]
                Vsz = orbital_paras[5][0] * t ** 3 + orbital_paras[5][1] * t ** 2 + orbital_paras[5][2] * t + orbital_paras[5][3]
                R = math.sqrt((Xs-Xp)**2+(Ys-Yp)**2+(Zs-Zp)**2)

                Fde = -2*((Xs-Xp)*(Vsx-Vpx)+(Ys-Yp)*(Vsy-Vpy)+(Zs-Zp)*(Vsz-Vpz))/(λ*R)
                T = t - Td
                
                Fd = doppler_d[0] + doppler_d[1]*T**1 + doppler_d[2]*T**2 + doppler_d[3]*T**3 + doppler_d[4]*T**4

                dt = 0.001
                T = T + dt
                Fd_dt = doppler_d[0] + doppler_d[1]*T**1 + doppler_d[2]*T**2 + doppler_d[3]*T**3 + doppler_d[4]*T**4
                Fd_dif = (Fd_dt - Fd)/dt

                # t = t + dt
                # Xs = orbital_paras[0][0] * t ** 3 + orbital_paras[0][1] * t ** 2 + orbital_paras[0][2] * t + orbital_paras[0][3]
                # Ys = orbital_paras[1][0] * t ** 3 + orbital_paras[1][1] * t ** 2 + orbital_paras[1][2] * t + orbital_paras[1][3]
                # Zs = orbital_paras[2][0] * t ** 3 + orbital_paras[2][1] * t ** 2 + orbital_paras[2][2] * t + orbital_paras[2][3]
                # Vsx = orbital_paras[3][0] * t ** 3 + orbital_paras[3][1] * t ** 2 + orbital_paras[3][2] * t + orbital_paras[3][3]
                # Vsy = orbital_paras[4][0] * t ** 3 + orbital_paras[4][1] * t ** 2 + orbital_paras[4][2] * t + orbital_paras[4][3]
                # Vsz = orbital_paras[5][0] * t ** 3 + orbital_paras[5][1] * t ** 2 + orbital_paras[5][2] * t + orbital_paras[5][3]
                # R = math.sqrt((Xs - Xp) ** 2 + (Ys - Yp) ** 2 + (Zs - Zp) ** 2)
                # Fde_dt = -2((Xs - Xp) * (Vsx - Vpx) + (Ys - Yp) * (Vsy - Vpy) + (Zs - Zp) * (Vsz - Vpz)) / (λ * R)

                delta_t = (Fde - Fd)/Fd_dif
                print(delta_t)
                t = t + delta_t

            print(delta_t)
            print("迭代次数：", iter_cnt)




    pass


if __name__ == '__main__':
    dem_resampled_path = r'F:\MicroWorkspace\Ortho\Output\TestDEM\resampled.tif'
    meta_file_path = r'F:\MicroWorkspace\Ortho\Input\4-GF3_KAS_FSII_020008_E113.2_N23.1_20200528_L1A_HHHV_L10004829485\GF3_KAS_FSII_020008_E113.2_N23.1_20200528_L1A_HHHV_L10004829485.meta.xml'
    indirect_pstn(dem_resampled_path, meta_file_path)
# #  1.创建文件对象
# f = open('cav_file.csv', 'w', encoding='utf-8-sig', newline="")
#
# #  2.基于文件对象构建csv写入对象
# csv_write = csv.writer(f)
#
# #  3.构建列表头
# csv_write.writerow(['学号', '班级', '姓名'])
#
# #  4.写入csv文件
# csv_write.writerow(['001', '2021.1', '张三'])
# csv_write.writerow(['002', '2021.1', '李四'])
# csv_write.writerow(['003', '2021.1', '王五'])
# csv_write.writerow(['004', '2021.1', '老六'])
#
# #  5.关闭文件
# f.close()
